﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using motion.Properties;
using Mahanet.Abyss.PathCalculation;

namespace motion
{
    public class To3DTranslator
    {
        public static Coordinate3D GetDetectionPointIn3DSpace(IMotionDetector detectorX, IMotionDetector detectorZ)
        {
            return To3DTranslator.GetPointIn3DSpace(detectorX.AngleX.Value, detectorZ.AngleX.Value, detectorZ.AngleY.Value);
        }

        public static Coordinate3D GetScreenPointIn3DSpace(int x, int z, int y, CameraWindow camWindow)
        {
            double detectionXAngle = ((double)x) / (((double)camWindow.Width) / Settings.Default.WidthDegrees);
            double detectionZAngle = ((double)z) / (((double)camWindow.Width) / Settings.Default.WidthDegrees);
            double detectionYAngle = ((double)(camWindow.Height - y)) / (((double)camWindow.Height) / Settings.Default.WidthDegrees);

            return To3DTranslator.GetPointIn3DSpace(detectionXAngle, detectionZAngle, detectionYAngle);
        }

        private static Coordinate3D GetPointIn3DSpace(double detectionXAngle, double detectionZAngle, double detectionYAngle)
        {
            // Calculate the prefixed X and Z values
            double preFixedXVal = (detectionXAngle / (Settings.Default.WidthDegrees / 2f)) * Settings.Default.FieldRadius;
            double preFixedZVal = (detectionZAngle / (Settings.Default.WidthDegrees / 2f)) * Settings.Default.FieldRadius;

            // Calculate the sign (+, -) with which to multiply parts of the delta calculation
            int signX = 1;
            int signZ = 1;

            if (preFixedZVal < 0)
            {
                signX = -1;
            }

            if (preFixedXVal < 0)
            {
                signZ = -1;
            }

            // Calculating the tan of the both angles
            double tanAlpha = Math.Tan(MathUtils.ConvertDegreesToRadians(Math.Abs(detectionZAngle)));
            double tanBeta = Math.Tan(MathUtils.ConvertDegreesToRadians(Math.Abs(detectionXAngle)));

            // Calculate the absolute value of the prefixed X and Z values
            double absolutePrefixedX = Math.Abs(preFixedXVal);
            double absolutePrefixedZ = Math.Abs(preFixedZVal);

            // Calculate the delta for both X and Z
            double zDelta = (tanAlpha * absolutePrefixedX + signX * tanAlpha * tanBeta * absolutePrefixedZ) /
                            (1 - signX * signZ * tanAlpha * tanBeta);

            double xDelta = tanBeta * absolutePrefixedZ + signZ * tanBeta * zDelta;


            // Calculate the "direction" sign
            float directionSignX = 1;

            if (preFixedXVal < 0)
            {
                directionSignX = -1;
            }

            float directionSignZ = 1;

            if (preFixedZVal < 0)
            {
                directionSignZ = -1;
            }

            // Calculate the correct X and Z values using the deltas found
            double correctXVal = preFixedXVal + directionSignX * signX * xDelta;
            double correctZVal = preFixedZVal + directionSignZ * signZ * zDelta;

            // Calculate the target's height. NOTE: This should be tested!
            double objectHeight = (Settings.Default.CameraDistanceFromTurret + correctXVal) * Math.Tan(MathUtils.ConvertDegreesToRadians(detectionYAngle));

            // Add the camera's height above the ground.
            objectHeight += Settings.Default.CameraDistanceAboveTurret;

            return new Coordinate3D(correctXVal, correctZVal, objectHeight);
        }

        private static double GetDetectionAngleX(double rawDetectionAngle)
        {
            if (rawDetectionAngle >= (Settings.Default.WidthDegrees / 2f))
            {
                return (rawDetectionAngle - (Settings.Default.WidthDegrees / 2f));
            }
            else
            {
                return (0 - ((Settings.Default.WidthDegrees / 2f) - rawDetectionAngle));
            }
        }

        private static double GetDetectionAngleY(double rawDetectionAngle)
        {
            if (Settings.Default.IsCameraAtMiddleOfYAxis)
            {
                if (rawDetectionAngle >= (MotionDetector4.s_AngleHeight / 2f))
                {
                    return (rawDetectionAngle - (MotionDetector4.s_AngleHeight / 2f));
                }
                else
                {
                    return (0 - ((MotionDetector4.s_AngleHeight / 2f) - rawDetectionAngle));
                }
            }
            else if (Settings.Default.IsUsingCameraHeightVar)
            {
                if (rawDetectionAngle >= MotionDetector4.s_ZeroAngle)
                {
                    return (rawDetectionAngle - MotionDetector4.s_ZeroAngle);
                }
                else
                {
                    return (0 - (MotionDetector4.s_ZeroAngle - rawDetectionAngle));
                }
            }
            else
            {
                return (rawDetectionAngle);
            }
        }
    }
}
